
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_motors_matrix.h
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "gp_motors_multicopter.h"
/*-----------------------------------macro------------------------------------*/
#define GP_MOTORS_MATRIX_YAW_FACTOR_CW   -1
#define GP_MOTORS_MATRIX_YAW_FACTOR_CCW   1
/*----------------------------------typedef-----------------------------------*/
typedef struct
{
    MotorsMC_HandleTypeDef tMotors;

    float               _roll_factor[GP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to roll
    float               _pitch_factor[GP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to pitch
    float               _yaw_factor[GP_MOTORS_MAX_NUM_MOTORS];  // each motors contribution to yaw (normally 1 or -1)
    float               _thrust_rpyt_out[GP_MOTORS_MAX_NUM_MOTORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range
    uint8_t             _test_order[GP_MOTORS_MAX_NUM_MOTORS];  // order of the motors in the test sequence
    motor_frame_class   _last_frame_class; // most recently requested frame class (i.e. quad, hexa, octa, etc)
    motor_frame_type    _last_frame_type; // most recently requested frame type (i.e. plus, x, v, etc)

    // motor failure handling
    float               _thrust_rpyt_out_filt[GP_MOTORS_MAX_NUM_MOTORS];    // filtered thrust outputs with 1 second time constant
    uint8_t             _motor_lost_index;  // index number of the lost motor

    uint16_t            _motors_mask;
}MotorsMat_HandleTypeDef;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/// Constructor
void          MotorsMatrix(MotorsMat_HandleTypeDef *pMotor, uint16_t loop_rate, uint16_t speed_hz);

void          MotorsMat_set_armed(MotorsMat_HandleTypeDef *pMotor, bool arm);

// init
void          MotorsMat_init(MotorsMat_HandleTypeDef *pMotor, motor_frame_class frame_class, motor_frame_type frame_type);

// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
void          MotorsMat_set_frame_class_and_type(MotorsMat_HandleTypeDef *pMotor, motor_frame_class frame_class, motor_frame_type frame_type);

// set update rate to motors - a value in hertz
// you must have setup_motors before calling this
void          MotorsMat_set_update_rate(MotorsMat_HandleTypeDef *pMotor, uint16_t speed_hz);

// output_test_seq - spin a motor at the pwm value specified
//  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
//  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void          MotorsMat_output_test_seq(MotorsMat_HandleTypeDef *pMotor, uint8_t motor_seq, int16_t pwm);

// output_test_num - spin a motor connected to the specified output channel
//  (should only be performed during testing)
//  If a motor output channel is remapped, the mapped channel is used.
//  Returns true if motor output is set, false otherwise
//  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
bool          MotorsMat_output_test_num(MotorsMat_HandleTypeDef *pMotor, uint8_t output_channel, int16_t pwm);

// output - sends commands to the motors
void          MotorsMat_output(MotorsMat_HandleTypeDef *pMotor);

// output_min - sets servos to neutral point with motors stopped
void          MotorsMat_output_min(MotorsMat_HandleTypeDef *pMotor);

// output_to_motors - sends minimum values out to the motors
void          MotorsMat_output_to_motors(MotorsMat_HandleTypeDef *pMotor);

// output - sends commands to the motors
void          MotorsMat_output_armed_stabilizing(MotorsMat_HandleTypeDef *pMotor);

// normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
void          MotorsMat_normalise_rpy_factors(MotorsMat_HandleTypeDef *pMotor);

// check for failed motor
void          MotorsMat_check_for_failed_motor(MotorsMat_HandleTypeDef *pMotor, float throttle_thrust_best);

// add_motor using raw roll, pitch, throttle and yaw factors
void          MotorsMat_add_motor_raw(MotorsMat_HandleTypeDef *pMotor, int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order);

// add_motor using just position and yaw_factor (or prop direction)
void          MotorsMat_add_motor4(MotorsMat_HandleTypeDef *pMotor, int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order);

// add_motor using separate roll and pitch factors (for asymmetrical frames) and prop direction
void          MotorsMat_add_motor5(MotorsMat_HandleTypeDef *pMotor, int8_t motor_num, float roll_factor_in_degrees, float pitch_factor_in_degrees, float yaw_factor, uint8_t testing_order);

// remove_motor
void          MotorsMat_remove_motor(MotorsMat_HandleTypeDef *pMotor, int8_t motor_num);

// configures the motors for the defined frame_class and frame_type
void          MotorsMat_setup_motors(MotorsMat_HandleTypeDef *pMotor, motor_frame_class frame_class, motor_frame_type frame_type);

/*
  disable the use of motor torque to control yaw. Used when an
  external mechanism such as vectoring is used for yaw control
*/
void          MotorsMat_disable_yaw_torque(MotorsMat_HandleTypeDef *pMotor);

// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
//  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t      MotorsMat_get_motor_mask(MotorsMat_HandleTypeDef *pMotor);

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



